#include "starrobot_robot.h"
starrobot_robot_object::starrobot_robot_object():
	baud_data(115200),lr_wheels_distance_(1.0),fr_wheels_distance_(1.0),wheel_diameter_(1.0),
	max_rpm_(366),pwm_bits_(8),pwm_max_(256),pwm_min_(-256),
    motor_reduction_ratio_(30),encoder_line_number_(13),counts_per_rev_(0),
    Direction_motor1_rotation_(1),Direction_motor2_rotation_(1),
    Direction_motor3_rotation_(1),Direction_motor4_rotation_(1),
    Direction_encoder1_value_(1),Direction_encoder2_value_(1),
    Direction_encoder3_value_(1),Direction_encoder4_value_(1),
    ultrasonic1_Enable_(0),ultrasonic2_Enable_(0),ultrasonic3_Enable_(0),
    servo1_Enable_(0),servo2_Enable_(0),servo3_Enable_(0),servo4_Enable_(0),
    initial_turning_angle_(0),max_steering_angle_(30),
    esc_median_value_(1500),a2_encoder_(0),emergency_stop_(0),
    k_p_(0.1),k_i_(0.1),k_d_(0.1),debug_message_(1),debug_base_(0),
    base_data_num(1),imu_data_num(1),motor_data_num(1),wheel_num(2),
    cmd_vel_sub_name_("cmd_vel"),swerve_sub_name_("initial_angle"),
    pid_sub_name_("pid"),servo_sub_name_("servo"),raw_vel_pub_name_("raw_vel"),
    raw_imu_pub_name_("raw_imu"),battery_pub_name_("battery"),sonar_pub_name_("sonar"),
    is_pid(false),is_servo(true),is_swerve(true),is_cmd(true),check_Acc_(1),check_Gyr_(1),
    check_Magne_(1),is_Read(false),is_RawVel(false),is_Battery(false),is_check(true)
{
	memset(&Reciver_Str, 0, sizeof(Reciver_Str));
	memset(&Send_Str, 0, sizeof(Send_Str));

	this->Send_Str.Sensor_Str.Header = PROTOCOL_HEADER;
	this->Send_Str.Sensor_Str.End_flag = PROTOCOL_END;

	// Get Luncher file define value 
  ros::NodeHandle nh_private("~");
  nh_private.param<std::string>("port", this->usart_port, "/dev/starrobotbase"); 
 	nh_private.param<int>("baud", this->baud_data, 115200); 
  nh_private.getParam("cmd_vel_sub_name",cmd_vel_sub_name_);
  nh_private.getParam("swerve_sub_name",swerve_sub_name_);
  nh_private.getParam("pid_sub_name",pid_sub_name_);
  nh_private.getParam("servo_sub_name",servo_sub_name_);
  nh_private.getParam("raw_vel_pub_name",raw_vel_pub_name_);
  nh_private.getParam("raw_imu_pub_name",raw_imu_pub_name_);
  nh_private.getParam("battery_pub_name",battery_pub_name_);
  nh_private.getParam("sonar_pub_name",sonar_pub_name_);
  
  raw_vel_pub  = nh_.advertise<starrobot_msgs::Velocities>(raw_vel_pub_name_, 1);
  raw_imu_pub  = nh_.advertise<starrobot_msgs::Imu>(raw_imu_pub_name_, 1);
  battery_pub  = nh_.advertise<starrobot_msgs::Battery>(battery_pub_name_, 1);
  pid_info_pub = nh_.advertise<starrobot_msgs::info_show>("pid_info", 10);
  
	cmd_vel_sub  = nh_.subscribe(cmd_vel_sub_name_, 10, &starrobot_robot_object::cmd_velCallback, this); 
	pid_sub      = nh_.subscribe(pid_sub_name_, 10, &starrobot_robot_object::pid_velCallback, this); 
	timer_Read   = nh_private.createTimer(ros::Duration(1.0/100), &starrobot_robot_object::ReadFormUart, this);
	timer_Battery  = nh_private.createTimer(ros::Duration(1.0/100), &starrobot_robot_object::PublisherBattery, this);
	timer_RawVel   = nh_private.createTimer(ros::Duration(1.0/50), &starrobot_robot_object::PublisherRawVel, this);
	timer_RawImu   = nh_private.createTimer(ros::Duration(1.0/100), &starrobot_robot_object::PublisherRawImu, this);
	/**open seril device**/
	try{
		 Robot_Serial.setPort(this->usart_port);
		 Robot_Serial.setBaudrate(this->baud_data);
		 serial::Timeout to = serial::Timeout::simpleTimeout(2000);
		 Robot_Serial.setTimeout(to);
		 Robot_Serial.open();
  }
	catch (serial::IOException& e){
		 ROS_ERROR_STREAM("[starrobot] Unable to open serial port, please check device or permission");
	}
	if(Robot_Serial.isOpen())
  {
	 	 ROS_INFO_STREAM("[starrobot] Successful opening of the serial port, data transmission started");
	}
  else
  {
     ROS_ERROR_STREAM("[starrobot] Unable to open serial port, please check device or permission");
	}
}

starrobot_robot_object::~starrobot_robot_object()
{
	Robot_Serial.close();
}
/***
 @ Description	-> cmd_vel Callback function
 @ Param		-> const geometry_msgs::Twist &twist_aux 
 @ Author		-> ChenZhouFu
 @ Date			-> 2020-4-1
 @ Function     -> starrobot_robot_object::cmd_velCallback(const geometry_msgs::Twist &twist_aux)
***/
void starrobot_robot_object::cmd_velCallback(const CmdConstPtr &twist_aux)
{
	// process callback function msgs
	Send_Str.Sensor_Str.X_Speed = twist_aux->linear.x;
	Send_Str.Sensor_Str.Y_Speed = twist_aux->linear.y;
	Send_Str.Sensor_Str.Z_Speed = twist_aux->angular.z;
	//boost::mutex::scoped_lock(write_mutex_);
	Robot_Serial.write(Send_Str.buffer, PROTOCL_DATA_SIZE);
	//ROS_INFO("cmd_velCallback");
}

/***
 @ Description	-> pid Callback function
 @ Param		-> const const starrobot_msgs::PID& pid
 @ Author		-> ChenZhouFu
 @ Date			-> 2020-4-1
 @ Function     -> starrobot_robot_object::pid_velCallback(const starrobot_msgs::PID& pid)
***/
void starrobot_robot_object::pid_velCallback(const PidConstPtr& pid)
{
	// process callback function msgs
	Send_Str.Sensor_Str.Kp_              = pid->p;
	Send_Str.Sensor_Str.Ki_              = pid->i;
	Send_Str.Sensor_Str.Kd_              = pid->d;
	Send_Str.Sensor_Str.PID_Debug_Enable = 1;
	is_pid = true;
	Robot_Serial.write(Send_Str.buffer, PROTOCL_DATA_SIZE);
}
/***
 @ Description	-> send and get stm32 board data
 @ Param		-> null
 @ Author		-> ChenZhouFu
 @ Date			-> 2019-03-10
 @ Function     -> bool starrobot_robot_object::ReadFormUart()
 @ return 		-> status
***/
void starrobot_robot_object::ReadFormUart(const ros::TimerEvent& e)
{	
	unsigned char CheckSumBuffer[1];
	Robot_Serial.read(Reciver_Str.buffer,PROTOCL_DATA_SIZE);
	if (Reciver_Str.Sensor_Str.Header == PROTOCOL_HEADER)
	{   
		if (Reciver_Str.Sensor_Str.End_flag == PROTOCOL_END)
		{ 
		    is_Read    = true;
		    is_RawVel  = true;
		    is_Battery = true;
		    
		}
	}
	else
	{ 
	  Robot_Serial.read(CheckSumBuffer,sizeof(CheckSumBuffer));
	  memset(Reciver_Str.buffer,0,sizeof(Reciver_Str.buffer));
	  ROS_ERROR("Starrobot Base Upload Data Error");
	} 
}

/***
 @ Description	-> Publisher RawVel
 @ Param		-> null
 @ Author		-> ChenZhouFu
 @ Date			-> 2019-03-10
 @ Function     -> starrobot_robot_object::PublisherRawVel(void)
 @ return 		-> NULL
***/
void starrobot_robot_object::PublisherRawVel(const ros::TimerEvent& e)
{ 
  if(is_RawVel)
  {
	raw_vel_msg.linear_x = Reciver_Str.Sensor_Str.X_Speed;
	raw_vel_msg.linear_y = Reciver_Str.Sensor_Str.Y_Speed;
	raw_vel_msg.angular_z = Reciver_Str.Sensor_Str.Z_Speed;
  }
  is_RawVel = false;
	raw_vel_pub.publish(raw_vel_msg);	
}

/***
 @ Description	-> Publisher RawImu()
 @ Param		-> null
 @ Author		-> ChenZhouFu
 @ Date			-> 2020-04-1
 @ Function     -> starrobot_robot_object::PublisherRawImu()
 @ return 		-> null
***/
void starrobot_robot_object::PublisherRawImu(const ros::TimerEvent& e)
{ 
  if(is_Read)
	{  
		check_Acc_   = Reciver_Str.Sensor_Str.check_Acc;
		check_Gyr_   = Reciver_Str.Sensor_Str.check_Gyr;
		check_Magne_ = Reciver_Str.Sensor_Str.check_Magne;
		//ROS_INFO("check_Acc_:%d check_Gyr_:%d check_Magne_:%d",check_Acc_,check_Gyr_,check_Magne_);
		if((!check_Acc_ || !check_Gyr_ || !check_Magne_)&&is_check)
		{
			ROS_WARN("Accelerometer NOT FOUND!");
			ROS_WARN("Gyroscope NOT FOUND!");
			ROS_WARN("Magnetometer NOT FOUND!");
			raw_imu_msg.linear_acceleration.x = 0.0;
			raw_imu_msg.linear_acceleration.y = 0.0;
			raw_imu_msg.linear_acceleration.z = 0.0;
			raw_imu_msg.angular_velocity.x = 0.0;
			raw_imu_msg.angular_velocity.y = 0.0;
			raw_imu_msg.angular_velocity.z = 0.0;
			raw_imu_msg.magnetic_field.x = 0.0;
			raw_imu_msg.magnetic_field.y = 0.0;
			raw_imu_msg.magnetic_field.z = 0.0;
			is_check = true;
		}
		else
		{ 
			
				raw_imu_msg.linear_acceleration.x = Reciver_Str.Sensor_Str.Link_Accelerometer.x;
				raw_imu_msg.linear_acceleration.y = Reciver_Str.Sensor_Str.Link_Accelerometer.y;
				raw_imu_msg.linear_acceleration.z = Reciver_Str.Sensor_Str.Link_Accelerometer.z;
				raw_imu_msg.angular_velocity.x = Reciver_Str.Sensor_Str.Link_Gyroscope.x;
				raw_imu_msg.angular_velocity.y = Reciver_Str.Sensor_Str.Link_Gyroscope.y;
				raw_imu_msg.angular_velocity.z = Reciver_Str.Sensor_Str.Link_Gyroscope.z;
				raw_imu_msg.magnetic_field.x = Reciver_Str.Sensor_Str.Link_Magnetometer.x;
				raw_imu_msg.magnetic_field.y = Reciver_Str.Sensor_Str.Link_Magnetometer.y;
				raw_imu_msg.magnetic_field.z = Reciver_Str.Sensor_Str.Link_Magnetometer.z;
				is_check = false;		  
		}				
	}
	is_Read = false;
	raw_imu_pub.publish(raw_imu_msg);
}

/***
 @ Description	-> Publisher Battery
 @ Param		-> null
 @ Author		-> ChenZhouFu
 @ Date			-> 2019-04-1
 @ Function     -> starrobot_robot_object::PublisherBattery()
 @ return 		-> NULL
***/
void starrobot_robot_object::PublisherBattery(const ros::TimerEvent& e)
{   
    if(is_Battery)
    {
			raw_bat_msg.voltage = Reciver_Str.Sensor_Str.Voltage;
			raw_bat_msg.electricity = Reciver_Str.Sensor_Str.Electricity;    
    }
    is_Battery = false;
		battery_pub.publish(raw_bat_msg);
		DebugEncoderDataShow();
}	
/***
 @ Description	-> Debug Encoder Data Show
 @ Param		-> null
 @ Author		-> ChenZhouFu
 @ Date			-> 2020-4-1
 @ Function     -> starrobot_robot_object::DebugEncoderDataShow()
 @ return 		-> NULL
***/
void starrobot_robot_object::DebugEncoderDataShow()
{  	
    if(debug_base_)
		{
				switch(wheel_num)
				{
						case 1:
							if(a2_encoder_)
								ROS_INFO("A2 Encoder: %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
							break;
						case 2:
							ROS_INFO("Encoder Left Front  1 : %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
							ROS_INFO("Encoder Right Front 2 : %lld",Reciver_Str.Sensor_Str.Encoder2_Value);
								break;
						case 3:
							ROS_INFO("Encoder Front  1 : %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
							ROS_INFO("Encoder Right  2 : %lld",Reciver_Str.Sensor_Str.Encoder2_Value);
							ROS_INFO("Encoder Left   3 : %lld",Reciver_Str.Sensor_Str.Encoder3_Value);
								break;
						case 4:
							ROS_INFO("Encoder Left Front  1 : %lld",Reciver_Str.Sensor_Str.Encoder1_Value);
							ROS_INFO("Encoder Right Front 2 : %lld",Reciver_Str.Sensor_Str.Encoder2_Value);
							ROS_INFO("Encoder Left Rear   3 : %lld",Reciver_Str.Sensor_Str.Encoder3_Value);
							ROS_INFO("Encoder Right Rear  4 : %lld",Reciver_Str.Sensor_Str.Encoder4_Value);
							break;
				}
		}
		/*raw_encoder_msg.MotorEncoder1 = Reciver_Str.Sensor_Str.Encoder1_Value;
		raw_encoder_msg.MotorEncoder2 = Reciver_Str.Sensor_Str.Encoder2_Value;
		raw_encoder_msg.MotorEncoder3 = Reciver_Str.Sensor_Str.Encoder3_Value;
		raw_encoder_msg.MotorEncoder4 = Reciver_Str.Sensor_Str.Encoder4_Value;*/
		info_show_msg.Motor1_Expectations = Reciver_Str.Sensor_Str.Motor1_Srpm;
		info_show_msg.Motor2_Expectations = Reciver_Str.Sensor_Str.Motor2_Srpm;
		info_show_msg.Motor3_Expectations = 0;
		info_show_msg.Motor4_Expectations = 0;
		info_show_msg.Motor1_Feedback			= Reciver_Str.Sensor_Str.Motor1_Crpm;
		info_show_msg.Motor2_Feedback			= Reciver_Str.Sensor_Str.Motor2_Crpm;
		info_show_msg.Motor3_Feedback			= 0;
		info_show_msg.Motor4_Feedback			= 0;
		info_show_msg.Motor1_Out_Pwm			= Reciver_Str.Sensor_Str.Motor1_OPwm;
		info_show_msg.Motor2_Out_Pwm			= Reciver_Str.Sensor_Str.Motor2_OPwm;
		info_show_msg.Motor3_Out_Pwm			= 0;
		info_show_msg.Motor4_Out_Pwm 			= 0;
		
		//if(is_pid)
			pid_info_pub.publish(info_show_msg);
		//else
			//encoder_pub.publish(raw_encoder_msg); 
}



